#include <iostream>
#include <ceres/ceres.h>
#include <Eigen/Core>
#include <fstream>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <opencv2/opencv.hpp>
#include <pcl_conversions/pcl_conversions.h>
#include <vector>
#include <string>

#include "common.h"
#include "result_verify.h"

using namespace std;

typedef pcl::PointXYZRGB PointType;
string lidar_path, photo_path, intrinsic_path, extrinsic_path;
int error_threshold;
vector<float> init;

struct InitialParameters
{
    bool fisheye_model;
    cv::Mat camera_mat; //内参
    cv::Mat dist_coeff;
};
InitialParameters _i_params;

class external_cali
{
public:
    external_cali(PnPData p, cv::Mat intinst, cv::Mat distort, bool fish)
    {
        assert((fish && distort.cols == 4) || (!fish && distort.cols == 5));
        pd = p;
        _intinst = intinst.clone();
        _distort = distort.clone();
        _isfish = fish;
    }

    template <typename T>
    bool operator()(const T *_q, const T *_t, T *residuals) const
    {
        Eigen::Quaternion<T> q_incre{_q[3], _q[0], _q[1], _q[2]};
        Eigen::Matrix<T, 3, 1> t_incre{_t[0], _t[1], _t[2]};

        Eigen::Matrix<T, 3, 1> p_l(T(pd.x), T(pd.y), T(pd.z));
        Eigen::Matrix<T, 3, 1> pt = q_incre.toRotationMatrix() * p_l + t_incre;
        // 按照线性模型，归一化(f=1)图像坐标系
        T tmpxC = pt[0] / pt[2];
        T tmpyC = pt[1] / pt[2];
        T r2 = tmpxC * tmpxC + tmpyC * tmpyC; //归一化平面的极坐标轴长的平方
        Eigen::Matrix<T, 2, 1> p_2;
        if (_isfish) // For fisheye camera model
        {
            T r1 = pow(r2, 0.5);    //归一化平面的极坐标轴长
            T a0 = ceres::atan(r1); //点和光轴的夹角

            T a1 = a0 * (T(1) + T(_distort.at<double>(0)) * pow(a0, 2) + T(_distort.at<double>(1)) * pow(a0, 4) + T(_distort.at<double>(2)) * pow(a0, 6) + T(_distort.at<double>(3)) * pow(a0, 8)); //畸变矫正，因镜头误差导致的新夹角，根据等距模型，也等于成像平面的矫正后新轴距
            //按照相似三角形计算矫正后的归一化平面点的坐标

            p_2[0] = (a1 / r1) * tmpxC;
            p_2[1] = (a1 / r1) * tmpyC;
        }
        else // For pinhole camera model
        {
            T tmpdist = T(1) + T(_distort.at<double>(0)) * r2 + T(_distort.at<double>(1)) * r2 * r2 +
                        T(_distort.at<double>(4)) * r2 * r2 * r2;
            p_2[0] = tmpxC * tmpdist + T(2) * T(_distort.at<double>(2)) * tmpxC * tmpyC +
                     T(_distort.at<double>(3)) * (r2 + T(2) * tmpxC * tmpxC);
            p_2[1] = tmpyC * tmpdist + T(_distort.at<double>(2)) * (r2 + T(2) * tmpyC * tmpyC) +
                     T(2) * T(_distort.at<double>(3)) * tmpxC * tmpyC;
        }
        //计算像素坐标
        p_2[0] = T(_intinst.at<double>(0, 0)) * p_2[0] + T(_intinst.at<double>(0, 2));
        p_2[1] = T(_intinst.at<double>(1, 1)) * p_2[1] + T(_intinst.at<double>(1, 2));

        residuals[0] = p_2[0] - T(pd.u);
        residuals[1] = p_2[1] - T(pd.v);

        return true;
    }

    static ceres::CostFunction *Create(PnPData p, cv::Mat intinst, cv::Mat distort, bool fish = false)
    {
        return (new ceres::AutoDiffCostFunction<external_cali, 2, 4, 3>(new external_cali(p, intinst, distort, fish)));
    }

private:
    PnPData pd;
    cv::Mat _intinst;
    cv::Mat _distort;
    bool _isfish;
};

int main(int argc, char **argv)
{
    bool isfish = false;
    // 输入点云、像素点对:    u       v      x     y       z
    vector<PnPData> pData;
    if (isfish)
    {
        //鱼眼
        pData.emplace_back(714, 262, 7.39484, 2.68911, 0.302916);
        pData.emplace_back(724, 358, 7.23878, 2.61548, -0.733356);
        pData.emplace_back(845, 342, 7.71027, 1.39676, -0.717918);
        pData.emplace_back(838, 251, 7.9235, 1.4763, 0.30288);
        pData.emplace_back(1156, 330, 9.54426, -2.78806, -0.578811);
        pData.emplace_back(1182, 299, 9.60982, -3.16476, -0.155239);
        pData.emplace_back(951, 279, 28.4222, 1.02161, 0.550671);
        pData.emplace_back(1003, 263, 12.4378, -0.630129, 0.361897);
        pData.emplace_back(1038, 289, 12.3828, -1.33181, -0.146821);
    }
    else
    {
        //针孔
        pData.emplace_back(60, 580, 12.485, 5.735, -0.108);
        pData.emplace_back(676, 502, 29.72, 4.63, 0.74);
        pData.emplace_back(1249, 490, 29.02, -3.55, 0.958);
        pData.emplace_back(1461, 497, 20.6013, -4.66, 0.548);
        pData.emplace_back(1803, 547, 13.406, -5.15, -0.027);
    }

    for (auto &p : pData)
    {
        cout << p.u << " " << p.v << " " << p.x << " " << p.y << " " << p.z << endl;
    }

    // 初始位姿 rpyxyz 0, -2, 1.584  0.05,0.6,-2.3
    Eigen::Vector3d eulerAngle(0, -2, 1.584);
    Eigen::Vector3d t(0.05, 0.6, -2.3);
    Eigen::AngleAxisd rollAngle(Eigen::AngleAxisd(eulerAngle(0), Eigen::Vector3d::UnitX()));
    Eigen::AngleAxisd pitchAngle(Eigen::AngleAxisd(eulerAngle(1), Eigen::Vector3d::UnitY()));
    Eigen::AngleAxisd yawAngle(Eigen::AngleAxisd(eulerAngle(2), Eigen::Vector3d::UnitZ()));
    Eigen::Quaterniond q = yawAngle * pitchAngle * rollAngle;
    double ext[7] = {q.x(), q.y(), q.z(), q.w(), t(0), t(1), t(2)};

    Eigen::Map<Eigen::Quaterniond> m_q = Eigen::Map<Eigen::Quaterniond>(ext);
    Eigen::Map<Eigen::Vector3d> m_t = Eigen::Map<Eigen::Vector3d>(ext + 4);

    ceres::LocalParameterization *q_parameterization = new ceres::EigenQuaternionParameterization();
    ceres::Problem problem;

    problem.AddParameterBlock(ext, 4, q_parameterization); //四元数并不是通过简单的加减乘除就可以进行运算，而是有一套自己的计算方法，因此这种情况必须手动定义加运算，在ceres中要采用ceres::LocalParameterization来定义运算
    problem.AddParameterBlock(ext + 4, 3);

    if (isfish)
    {
        //鱼眼
        double camera_mat[9] = {4.7744114021272674e+02, 0., 9.6438755066918418e+02, 0., 4.7991906955214409e+02, 5.3697598079257989e+02, 0, 0, 1};
        cv::Mat(3, 3, CV_64F, &camera_mat).copyTo(_i_params.camera_mat); //内参
        double dist_coeff[4] = {6.2849888891468336e-02, -2.4029595614050239e-02, 9.9529184047524553e-03, -2.2128438750643269e-03};
        cv::Mat(1, 4, CV_64F, &dist_coeff).copyTo(_i_params.dist_coeff); //畸变系数
    }
    else
    {
        //小孔
        double camera_mat[9] = {1949.7533168494813, 0., 996.8739160943692, 0., 1943.351745233424,
                                462.66729438851775, 0, 0, 1};
        cv::Mat(3, 3, CV_64F, &camera_mat).copyTo(_i_params.camera_mat); //内参
        double dist_coeff[5] = {-0.5434736162932838,
                                0.33114769881879574,
                                0.0026528927512422837,
                                -0.0025526579297368763};
        cv::Mat(1, 5, CV_64F, &dist_coeff).copyTo(_i_params.dist_coeff); //畸变系数
    }

    for (auto val : pData)
    {
        ceres::CostFunction *cost_function;
        cost_function = external_cali::Create(val, _i_params.camera_mat, _i_params.dist_coeff, isfish);
        problem.AddResidualBlock(cost_function, NULL, ext, ext + 4);
    }

    ceres::Solver::Options options;
    options.linear_solver_type = ceres::DENSE_SCHUR;
    options.minimizer_progress_to_stdout = true;
    options.trust_region_strategy_type = ceres::LEVENBERG_MARQUARDT;

    ceres::Solver::Summary summary;
    ceres::Solve(options, &problem, &summary);
    cout << summary.BriefReport() << endl;

    // Eigen::Matrix3d rot = m_q.toRotationMatrix();
    // writeExt(extrinsic_path, rot, m_t);
    // cout << rot << endl;
    // cout << m_t << endl;
    cout << "初始位姿\n"
         << "旋转ypr\n"
         << q.matrix().eulerAngles(2, 1, 0) << "\n平移xyz\n"
         << t << endl;
    cout << "结果位姿\n"
         << "旋转ypr\n"
         << m_q.matrix().eulerAngles(2, 1, 0) << "\n平移xyz\n"
         << m_t << endl;

    // cout << "Use the extrinsic result to reproject the data" << endl;
    // float error[2] = {0, 0};
    // getUVError(intrinsic_path, extrinsic_path, lidar_path, photo_path, error, error_threshold);

    // cout << "u average error is: " << error[0] << endl;
    // cout << "v average error is: " << error[1] << endl;
    // if (error[0] + error[1] < error_threshold) {
    //     cout << endl << "The reprojection error smaller than the threshold, extrinsic result seems ok" << endl;
    // }
    // else {
    //     cout << endl << "The reprojection error bigger than the threshold, extrinsic result seems not ok" << endl;
    // }

    return 0;
}
